#!/usr/bin/env python
# -*- coding:UTF-8 -*-
from __future__ import print_function
import serial
import math


def normalize_angle(angle):
    """标准化角度到0-360度范围"""
    return angle % 360.0


def angle_difference(a, b):
    """计算两个角度间的最小差值（考虑圆周）[1,5](@ref)"""
    diff = abs(a - b)
    return min(diff, 360 - diff)


if __name__ == '__main__':
    last_angle = 0
    ser = serial.Serial('/dev/ttyUSB0', 230400)  # Ubuntu
    # ser = serial.Serial("COM13", 230400, timeout=5)  # Windows

    print("物体距离检测系统已启动（单位：厘米）")
    print("{:<8} {:<8} {:<10}".format("点号", "角度(°)", "距离(cm)"))
    print("-" * 30)

    while True:
        data = ser.read(2)
        if data[0] == 0x54 and data[1] == 0x2C:  # 帧头检测
            data = ser.read(45)  # 读取完整数据帧

            # 解析起始角度和结束角度
            start_angle = (data[3] * 256 + data[2]) / 100.0
            end_angle = (data[41] * 256 + data[40]) / 100.0

            # 标准化角度并计算角度差
            start_angle = normalize_angle(start_angle)
            end_angle = normalize_angle(end_angle)
            angle_diff = end_angle - start_angle
            if angle_diff < 0:
                angle_diff += 360.0  # 处理0°-360°边界情况

            # 检测是否完成新的一圈扫描
            if angle_difference(last_angle, start_angle) > 100:
                print("\n" + "*" * 50)
                print(" 新扫描周期开始 ".center(50, "*"))
                print("*" * 50 + "\n")
                print("{:<8} {:<8} {:<10}".format("点号", "角度(°)", "距离(cm)"))
                print("-" * 30)
            last_angle = start_angle

            # 解析并转换12个测量点的距离（毫米→厘米）
            for i in range(12):
                # 计算当前点的角度
                point_angle = normalize_angle(start_angle + angle_diff * i / 11.0)

                # 提取距离数据并转换为厘米（毫米÷10）[1,2,3,4,5](@ref)
                idx = 4 + i * 3
                distance_mm = data[idx + 1] * 256 + data[idx]  # 原始毫米单位
                distance_cm = distance_mm / 10.0  # 毫米→厘米转换

                # 打印格式化的距离信息（保留1位小数）
                print("{:<8} {:<8.2f} {:<10.1f}".format(i + 1, point_angle, distance_cm))
            print("-" * 30)